#include "matlabc.h"


FitCircle CircleFit1(const std::vector<cv::Point>& points)
{
    int n = points.size();

    //构造矩阵A和向量y
    cv::Mat A(n,3,CV_64F);
    cv::Mat y(n,1,CV_64F);



    for (int i = 0; i < n; i++)
    {
        A.at<double>(i,0) = points[i].x;
        A.at<double>(i,1) = points[i].y;
        A.at<double>(i,2) = 1;
        y.at<double>(i,0) = -(points[i].x * points[i].x + points[i].y * points[i].y);
    }

    //解线性方程组Ax = y  圆的一般方程 x^2 + y^2 + a*x + b*y + c = 0
    cv::Mat x;
    cv::solve(A,y,x,cv::DECOMP_SVD);

    double a = x.at<double>(0,0);
    double b = x.at<double>(1,0);
    double c = x.at<double>(2,0);

    double x0 = -a / 2;
    double y0 = -b / 2;
    double r0 = std::sqrt((a * a + b * b) / 4 - c);

    //计算圆整度
//    std::vector<double> distances(n);
//    double sum_dist = 0.0;
//    for (int i = 0; i < n; i++)
//    {
//        distances[i] = std::sqrt((points[i].x - x0) * (points[i].x - x0) + (points[i].y - y0) + (points[i].y - y0));
//        sum_dist +=distances[i];
//    }
//    double mean_dist = sum_dist / n;

//    double sum_sq_diff = 0.0;
//    for (double dist : distances)
//    {
//        sum_sq_diff += (dist - mean_dist) * (dist - mean_dist);
//    }

//    double std_dev = std::sqrt(sum_sq_diff / n);
//    double roundness = 1 - std_dev / mean_dist;

    return {x0,y0,r0,0};
}

FitCircle CircleFit1(const std::vector<float>& x_pos,const std::vector<float>& y_pos)
{
    int n = x_pos.size();

    //构造矩阵A和向量y
    cv::Mat A(n,3,CV_64F);
    cv::Mat y(n,1,CV_64F);

    for (int i = 0; i < n; i++)
    {
        A.at<double>(i,0) = x_pos[i];
        A.at<double>(i,1) = y_pos[i];
        A.at<double>(i,2) = 1;
        y.at<double>(i,0) = -(x_pos[i] * x_pos[i] + y_pos[i] * y_pos[i]);
    }

    //解线性方程组Ax = y  圆的一般方程 x^2 + y^2 + a*x + b*y + c = 0
    cv::Mat x;
    cv::solve(A,y,x,cv::DECOMP_SVD);

    double a = x.at<double>(0,0);
    double b = x.at<double>(1,0);
    double c = x.at<double>(2,0);

    double x0 = -a / 2;
    double y0 = -b / 2;
    double r0 = std::sqrt((a * a + b * b) / 4 - c);

    //计算圆整度
    std::vector<double> distances(n);
    double sum_dist = 0.0;
    for (int i = 0; i < n; i++)
    {
        distances[i] = std::sqrt((x_pos[i] - x0) * (x_pos[i] - x0) + (y_pos[i] - y0) + (y_pos[i] - y0));
        sum_dist +=distances[i];
    }
    double mean_dist = sum_dist / n;

    double sum_sq_diff = 0.0;
    for (double dist : distances)
    {
        sum_sq_diff += (dist - mean_dist) * (dist - mean_dist);
    }

    double std_dev = std::sqrt(sum_sq_diff / n);
    double roundness = 1 - std_dev / mean_dist;

    return {x0,y0,r0,roundness};
}

//粗定位
LocCircleResult CoarseLocation1(const cv::Mat& im)
{

    cv::Mat g,g1;
    cv::Mat labeled;
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;

    cv::medianBlur(im,im,3);

    cv::threshold(im,g,0,255,cv::THRESH_BINARY | cv::THRESH_OTSU);

    cv::bitwise_not(g,g1);

    cv::Mat labels,stats,centroids;
    int num_labels = cv::connectedComponentsWithStats(g1,labels,stats,centroids);
//    for (int i = 1; i < num_labels; i++)
//    {
//        int area = stats.at<int>(i,cv::CC_STAT_AREA);
//        if (area < 40000)
//        {
//            g1.setTo(0,labels == i);
//        }
//    }
    tbb::parallel_for(1,num_labels,1,[&](int i)
    {
//        int area = stats.at<int>(i,cv::CC_STAT_AREA);
//        if (area < 40000)
//        {
//            g1.setTo(0,labels == i);
//        }
        if (stats.at<int>(i,cv::CC_STAT_AREA) < 40000)
        {
    g1.setTo(0,labels == i);
}

    });

    std::cout << "num_labels: " << num_labels << std::endl;

    cv::Mat se = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(7,7));
    cv::morphologyEx(g1,g1,cv::MORPH_OPEN,se);
    cv::morphologyEx(g1,g1,cv::MORPH_CLOSE,se);


    //查找轮廓
    cv::findContours(g1,contours,hierarchy,cv::RETR_LIST,cv::CHAIN_APPROX_SIMPLE);
    std::cout << "contours_size: " << contours.size() << std::endl;
    std::vector<double> xs0,ys0,Rs0;

    int thr_num = 0;
    for (size_t k = 0; k < contours.size(); k++)
    {
        if (contours[k].size() < 2000)
        {
            continue;
        }

        FitCircle fitted_circle = CircleFit1(contours[k]);
        xs0.push_back(fitted_circle.x);
        ys0.push_back(fitted_circle.y);
        Rs0.push_back(fitted_circle.r);
        thr_num++;
    }
    std::cout << "thr_num: " << thr_num << std::endl;

    //8.计算平均圆心坐标和指定半径
    double mean_x = std::accumulate(xs0.begin(),xs0.end(),0.0) / xs0.size();
    double mean_y = std::accumulate(ys0.begin(),ys0.end(),0.0) / ys0.size();

    int center_x = static_cast<int>(std::round(mean_x));
    int center_y = static_cast<int>(std::round(mean_y));
    int ring_ex_rad = static_cast<int>(std::round(Rs0[2]));
    int ring_in_rad = static_cast<int>(std::round(Rs0[1]));
    int circle_rad = static_cast<int>(std::round(Rs0[0]));

    LocCircleResult result = {center_x,center_y,circle_rad,ring_ex_rad,ring_in_rad};
    return result;

}

//粗定位
LocCircleResult CoarseLocation(const cv::Mat& im)
{

    cv::Mat g,g1;
    cv::Mat labeled;
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;

    cv::medianBlur(im,im,3);

    cv::threshold(im,g,0,255,cv::THRESH_BINARY | cv::THRESH_OTSU);

    cv::bitwise_not(g,g1);

    cv::Mat se = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(7,7));

//    cv::morphologyEx(g1,g1,cv::MORPH_OPEN,se);
//    cv::morphologyEx(g1,g1,cv::MORPH_CLOSE,se);


    //查找轮廓
    cv::findContours(g1,contours,hierarchy,cv::RETR_LIST,cv::CHAIN_APPROX_SIMPLE);
    //cv::findContours(g1,contours,hierarchy,cv::RETR_LIST,cv::CHAIN_APPROX_NONE);

//    for (size_t i = 0; i < contours.size(); i++)
//    {
//        if (cv::contourArea(contours[i]) < 40000)
//        {
//            contours.erase(contours.begin() + i);
//        }
//    }
    contours.erase(std::remove_if(contours.begin(),contours.end(),
[](const std::vector<cv::Point>& c){return cv::contourArea(c) < 60000;}),contours.end());


    cv::morphologyEx(g1,g1,cv::MORPH_OPEN,se);
    cv::morphologyEx(g1,g1,cv::MORPH_CLOSE,se);

    std::cout << "contours_size: " << contours.size() << std::endl;


    std::vector<double> xs0,ys0,Rs0;

    int thr_num = 0;
    for (size_t k = 0; k < contours.size(); k++)
    {
        if (contours[k].size() < 2000)
        {
            continue;
        }

        FitCircle fitted_circle = CircleFit1(contours[k]);
        xs0.push_back(fitted_circle.x);
        ys0.push_back(fitted_circle.y);
        Rs0.push_back(fitted_circle.r);
        thr_num++;
    }
    std::cout << "thr_num: " << thr_num << std::endl;

    //8.计算平均圆心坐标和指定半径
    double mean_x = std::accumulate(xs0.begin(),xs0.end(),0.0) / xs0.size();
    double mean_y = std::accumulate(ys0.begin(),ys0.end(),0.0) / ys0.size();

    int center_x = static_cast<int>(std::round(mean_x));
    int center_y = static_cast<int>(std::round(mean_y));
    int ring_ex_rad = static_cast<int>(std::round(Rs0[2]));
    int ring_in_rad = static_cast<int>(std::round(Rs0[1]));
    int circle_rad = static_cast<int>(std::round(Rs0[0]));

    LocCircleResult result = {center_x,center_y,circle_rad,ring_ex_rad,ring_in_rad};
    return result;

}
//设置圆模板
cv::Mat CreateCircleTemplate(int radius,int wid)
{
    int size = radius * 2 + 2 * wid;
    cv::Mat circleTemplate = cv::Mat::zeros(cv::Size(size,size),CV_32F);

    cv::Point center(size / 2,size / 2);
    cv::circle(circleTemplate,center,radius,cv::Scalar(1),-1);
    return circleTemplate ;

}

//设置圆环模板
cv::Mat CreateRingTemplate(int ring_ex_rad,int ring_in_rad,int wid)
{
    int size = ring_ex_rad * 2 + 2 * wid;
    cv::Mat ringTemplate = cv::Mat::zeros(cv::Size(size,size),CV_32F);

    cv::Point center(size / 2,size / 2);

    cv::circle(ringTemplate,center,ring_ex_rad,cv::Scalar(1),-1);
    cv::circle(ringTemplate,center,ring_in_rad,cv::Scalar(0),-1);

    return ringTemplate;
}

//裁剪
cv::Mat CropCircleImage(const cv::Mat& image,const LocCircleResult& L,int cut_wid)
{
    int top = L.center_y - L.circle_rad - cut_wid;
    int bottom = L.center_y + L.circle_rad + cut_wid;
    int left = L.center_x - L.circle_rad - cut_wid;
    int right = L.center_x + L.circle_rad + cut_wid;

    top = std::max(top,0);
    bottom = std::min(bottom,image.rows);
    left = std::max(left,0);
    right = std::min(right,image.cols);
    cv::Rect cropRegion(left,top,right - left,bottom - top);//定义裁剪区域
    cv::Mat croped_image = image(cropRegion);

    cv::Mat im_circle_lo = cv::Mat::zeros(croped_image.size(),CV_8U);
    cv::Point center(croped_image.cols / 2,croped_image.rows / 2);
    cv::circle(im_circle_lo,center,L.circle_rad,cv::Scalar(255),-1);

    //转换裁剪图像与模板 为浮点型并且归一化
    cv::Mat croppedF,maskF;
    croped_image.convertTo(croppedF,CV_32F,1.0 / 255);
    im_circle_lo.convertTo(maskF,CV_32F,1.0 / 255);
    //圆内保持原图 圆外为0
    cv::Mat resultF = croppedF.mul(maskF);

    //转换回 cv_8u(0-255)
    cv::Mat result;
    resultF.convertTo(result,CV_8U,255);

    return result;

}
cv::Mat CropRingImage(const cv::Mat& image,const struct LocCircleResult& L,int cut_wid)
{

    int top = L.center_y - L.ring_ex_rad - cut_wid;
    int bottom = L.center_y + L.ring_ex_rad + cut_wid;
    int left = L.center_x - L.ring_ex_rad - cut_wid;
    int right = L.center_x + L.ring_ex_rad + cut_wid;

    top = std::max(top,0);
    bottom = std::min(bottom,image.rows);
    left = std::max(left,0);
    right = std::min(right,image.cols);
    cv::Rect cropRegion(left,top,right - left,bottom - top);//定义裁剪区域
    cv::Mat croped_image = image(cropRegion);

    cv::Mat im_ring_lo = cv::Mat::zeros(croped_image.size(),CV_8U);
    cv::Point center(croped_image.cols / 2,croped_image.rows / 2);
    cv::circle(im_ring_lo,center,L.ring_ex_rad,cv::Scalar(255),-1);
    cv::circle(im_ring_lo,center,L.ring_in_rad,cv::Scalar(0),-1);

    //转换裁剪图像与模板 为浮点型并且归一化
    cv::Mat croppedF,maskF;
    croped_image.convertTo(croppedF,CV_32F,1.0 / 255);
    im_ring_lo.convertTo(maskF,CV_32F,1.0 / 255);
    //圆内保持原图 圆外为0
    cv::Mat resultF = croppedF.mul(maskF);

    //转换回 cv_8u(0-255)
    cv::Mat result;
    resultF.convertTo(result,CV_8U,255);

    return result;

}

Coeff PlaneSolu(const std::vector<cv::Point3f>& points_in)
{
    //构建矩阵A和z
    int n = points_in.size();
    cv::Mat A(n,3,CV_32F);
    cv::Mat z(n,1,CV_32F);
    for (int i = 0; i < n; i++)
    {
        A.at<float>(i,0) = points_in[i].x;//裁剪图片横坐标
        A.at<float>(i,1) = points_in[i].y;//裁剪图片纵坐标
        A.at<float>(i,2) = 1.0f;
        z.at<float>(i,0) = points_in[i].z;//z坐标 灰度值
    }

//    tbb::parallel_for(tbb::blocked_range<int>(0,n),[&](const tbb::blocked_range<int>& r)
//    {
//          for (int i = r.begin(); i != r.end(); ++i)
//          {
//            A.at<float>(i,0) = points_in[i].x;//裁剪图片横坐标
//            A.at<float>(i,1) = points_in[i].y;//裁剪图片纵坐标
//            A.at<float>(i,2) = 1.0f;
//            z.at<float>(i,0) = points_in[i].z;//z坐标 灰度值
//          }
//    });

    struct Coeff C;
    cv::Mat coeffs;
    cv::solve(A,z,coeffs,cv::DECOMP_SVD);
    C.a0 = coeffs.at<float>(0,0);
    C.a1 = coeffs.at<float>(1,0);
    C.a2 = coeffs.at<float>(2,0);

    return C;
}

void ExtractNZero(const cv::Mat& image,int center_x,int center_y,int radius,int cut_wid,std::vector<cv::Point3f>& points3f)
{
//    tbb::concurrent_vector<Point> concpoints;
//    tbb::concurrent_vector<float> concvalues;

//    tbb::parallel_for(0,image.rows,[&](int y)
//                      {
//                          for (int x = 0; x < image.cols; ++x)
//                          {
//                              uchar pixel = image.at<uchar>(y,x);
//                              if (pixel != 0)
//                              {
////                                  concpoints.push_back(Point(x,y));
////                                  concvalues.push_back(static_cast<float>(pixel));
//                                  points3f.push_back(Point3f(static_cast<float>(x),
//                                                    static_cast<float>(y),
//                                                    static_cast<float>(pixel)));
//                              }
//                          }

//                      });

    int x_start = max(0, center_x - radius - cut_wid);
    //int x_end = min(image.cols - 1, L.center_x + L.circle_rad + cut_wid);
    int y_start = max(0, center_y - radius - cut_wid);
    //int y_end = min(image.rows - 1, L.center_y + L.circle_rad + cut_wid);

//    std::cout << "x_start: " << x_start << std::endl;
//    std::cout << "y_start: " << y_start << std::endl;

//    tbb::parallel_for(0,image.rows,[&](int y)
//                      {
//                          for (int x = 0; x < image.cols; ++x)
//                          {
//                              uchar pixel = image.at<uchar>(y,x);
//                              if (pixel != 0)
//                              {
////                                  concpoints.push_back(Point(x,y));
////                                  concvalues.push_back(static_cast<float>(pixel));
//                                    points3f.push_back(Point3f(static_cast<float>(x + x_start),
//                                                    static_cast<float>(y + y_start),
//                                                    static_cast<float>(pixel)));
//                              }
//                          }

//                      });


//    for (int y = 0; y <= image.rows; ++y)
//    {
//        for (int x = 0;x <= image.cols; ++x)
//        {
//            uchar pixel = image.at<uchar>(y,x);
//            if (pixel != 0)
//            {
//                points3f.push_back(cv::Point3f(static_cast<float>(x + x_start),
//                                           static_cast<float>(y + y_start),
//                                           static_cast<float>(pixel)));
//            }
//        }
//    }

//    points.assign(concpoints.begin(),concpoints.end());
//    values.assign(concvalues.begin(),concvalues.end());

    vector<Point> nonzero;
    cv::findNonZero(image,nonzero);
    for (size_t i = 0; i < nonzero.size(); i++)
    {
        Point pt = nonzero[i];
        uchar grayvalue = image.at<uchar>(pt);
        points3f.push_back(Point3f(static_cast<float>(pt.x + x_start),
                                   static_cast<float>(pt.y + y_start),
                                   static_cast<float>(grayvalue)));
    }



}

cv::Mat CorrectGray(const cv::Mat& image,const struct Coeff& coeff,cv::Mat& im_lo,int center_x,int center_y,int radius,int cut_wid)
{
    cv::Mat xx_circle = cv::Mat::zeros(image.size(),CV_32F);
    cv::Mat yy_circle = cv::Mat::zeros(image.size(),CV_32F);

    int x_start = max(0, center_x - radius - cut_wid);
    //int x_end = min(image.cols - 1, L.center_x + L.circle_rad + cut_wid);
    int y_start = max(0, center_y - radius - cut_wid);

    //填充X Y
    for (int y = 0; y < image.rows; ++y)
    {
        for (int x = 0; x < image.cols; ++x)
        {
            xx_circle.at<float>(y,x) = static_cast<float>(x + x_start);
            yy_circle.at<float>(x,y) = static_cast<float>(y + y_start);
        }
    }

    cv::Mat plane = coeff.a0 * xx_circle + coeff.a1 * yy_circle + coeff.a2;
     plane.convertTo(plane,CV_32F);

//    for (int r = 0; r < 1; ++r)
//    {
//        const int *ptr = plane.ptr<int>(r);
//        for (int c = 0; c < 902; ++c)
//        {
//            std::cout << ptr[c] << ",";
//        }
//        std::cout << std::endl;
//    }
    double min_value;
    cv::minMaxLoc(plane,&min_value);

    //图像值减去平面值 然后加上平面上最小值
    cv::Mat corrected = image.clone();
    corrected.convertTo(corrected,CV_32F);
    //corrected = corrected - plane + cv::min(plane);
    corrected = corrected - plane + min_value;

//    for (int r = 0; r < 2; ++r)
//    {
//        const int *ptr = corrected.ptr<int>(r);
//        for (int c = 0; c < 902; ++c)
//        {
//            std::cout << ptr[c] << ",";
//        }
//        std::cout << std::endl;
//    }


    cv::Mat mask;
    im_lo.convertTo(mask,CV_32F,1.0 / 255);

    cv::Mat result = corrected.mul(mask);

    return result;

}

void ProcessInterp(const cv::Mat& temp,std::vector<float>& fitx_pos,std::vector<float>& fity_pos,int center_x,int center_y,int radius,int cut_wid)
{
    float lower_percentile = 0.8f;
    float upper_percentile = 0.95f;
    float den = 0.01f;

    std::cout << "temp.size: " << temp.size() << std::endl;
//    cv::Mat line = temp.row(0);
//    float val1 = line.at<float>(0,0);
//    float val2 = line.at<float>(0,1);
//    std::cout << "val1: " << val1 << " " << "val2: " << val2 << std::endl;


    std::vector<float> thr;
    for (float t = lower_percentile; t <= upper_percentile; t += den)
    {
        thr.push_back(t);
    }

//    for (auto th : thr)
//    {
//        std::cout << "thr: " << th << std::endl;
//    }

    int x_start = max(0, center_x - radius - cut_wid);
    //int x_end = min(image.cols - 1, L.center_x + L.circle_rad + cut_wid);
    int y_start = max(0, center_y - radius - cut_wid);

    //计算模板temp 行列索引
    std::vector<float> index_x(temp.cols),index_y(temp.rows);
    for (int i = 0; i < temp.cols; ++i) index_x[i] = i + x_start;
    for (int i = 0; i < temp.rows; ++i) index_y[i] = i + y_start;

//    std::cout << "index_x_start: " << index_x[0] << std::endl;
//    std::cout << "index_y_start: " << index_y[0] << std::endl;
//    std::cout << "index_x_end: " << index_x[temp.cols - 1] << std::endl;
//    std::cout << "index_y_end: " << index_y[temp.rows - 1] << std::endl;

    //存储插值后的点
    std::vector<float> interp_x,interp_y;

    //遍历阈值
    for (size_t i = 0; i < thr.size(); ++i)
    {
        float tem_thr = thr[i];
        int pot_num = 0;

        //x方向插值
        for (int line_num = 0;line_num < temp.rows; ++line_num)
        {
            //获取当前行数据q
            //x方向插值
            cv::Mat line = temp.row(line_num);

            //std::cout << "line.size: " << line.size() << std::endl;
            for (int x_num = 0; x_num < line.cols - 1; ++x_num)
            {
                float val1 = line.at<float>(0,x_num);
                float val2 = line.at<float>(0,x_num + 1);
                if ((val1 - tem_thr) * (val2 - tem_thr) < 0)
                {
                    //std::cout << "(val1 - tem_thr) * (val2 - tem_thr)_x_num: " << x_num << " ," <<  (val1 - tem_thr) * (val2 - tem_thr) << std::endl;
                    //线性插值
                    float n = index_x[x_num] + (tem_thr - val1) / (val2 - val1);
                    //float n = index_x[x_num] + (val2 - val1) * (tem_thr - index_x[x_num]);
                    float g = index_y[line_num];
                    interp_x.push_back(n);
                    interp_y.push_back(g);
                    pot_num ++;

                }
            }

        }
//        std::cout << "interp_x.size(): " << interp_x.size() << std::endl;
//        std::cout << "pot_num: " << pot_num << std::endl;

        //y向插值
        for (int line_num = 0;line_num < temp.cols; ++line_num)
        {
            //获取当前行数据
            //y方向插值 应该取行
            cv::Mat line = temp.col(line_num);

            for (int y_num = 0; y_num < line.rows - 1; ++y_num)
            {
                float val1 = line.at<float>(y_num,0);
                float val2 = line.at<float>(y_num + 1,0);
                if ((val1 - tem_thr) * (val2 - tem_thr) < 0)
                {
                    //线性插值
                    float n = index_x[line_num] ;
                    float g = index_y[y_num] + (tem_thr - val1) / (val2 - val1);
                    interp_x.push_back(n);
                    interp_y.push_back(g);
                }
            }
        }

        FitCircle fitcircle = CircleFit1(interp_x,interp_y);

//        for (auto inter_x : interp_x)
//        {
//            std::cout << inter_x << ",";
//        }
//        std::cout << std::endl;



        fitx_pos.push_back(fitcircle.x);
        fity_pos.push_back(fitcircle.y);

        //std::cout << "interp_x_size: " << interp_x.size() << std::endl;
    //清空 interp_x interp_y 防止一直push
//        interp_x = std::vector<float>();
//        interp_y = std::vector<float>();
        //std::cout << "interp_x_size: " << interp_x.size() << std::endl;
    }


}

double Mean(std::vector<float> &vec)
{
    double sum = std::accumulate(std::begin(vec),std::end(vec),0.0);
    double mean = sum / vec.size();

    return mean;
}

//matlabc::matlabc()
//{

//}
